/* 
 * File:   Module.cpp
 * Author: Piotrek
 * 
 * Created on 24 kwiecień 2010, 22:16
 */
#include "stdafx.h"
#include "Module.h"
#include "Robot.h"

#include <math.h>

/// jednostkowa macierz transformacji
double I[3][3]={{1,0,0},{0,1,0},{0,0,1}};
/// zerowy wektor translacji
double T0[3]={0, 0, 0};

const float M_PI = (float)System::Math::PI;

Module::Module(char* filepath): R(0.0f), X(0.0f), Y(0.0f), Z(0.0f)
{
	int num = 128;
    triangles.reserve(num);	

    readTrianglesFromStl(filepath);
}

Module::Module(char * filepath, float x0, float y0, float z0, char ax): R(0.0f), X(x0), Y(y0), Z(z0)
{	
    axis=ax;	
	int num = 128;
    triangles.reserve(num);

    readTrianglesFromStl(filepath);

}


Module::Module(float x0, float y0, float z0, char ax): R(0.0f)
{
	X = x0;
	Y = y0;
	Z = z0;
    axis=ax;	
}

Module::Module(float X, float Y, float Z, float A, float B, float C, int direction)
{
	this->X = X;
	this->Y = Y;
	this->Z = Z;
	this->R = 0;

	float n[3];

	float cordinates[8][3]={{0,0,0},
							{0,0,C},
							{0,B,0},
							{0,B,C},
							{A,0,0},
							{A,0,C},
							{A,B,0},
							{A,B,C}
	};
	

	n[0]=-1;	n[1]=0;	n[2]=0;
	triangles.push_back(Triangle(n, cordinates[0], cordinates[1], cordinates[2], direction));	
	triangles.push_back(Triangle(n, cordinates[3], cordinates[1], cordinates[2], direction));

	n[0]=1;	n[1]=0;	n[2]=0;
	triangles.push_back(Triangle(n, cordinates[4], cordinates[5], cordinates[6], direction));	
	triangles.push_back(Triangle(n, cordinates[7], cordinates[5], cordinates[6], direction));


	n[0]=0;	n[1]=-1;	n[2]=0;
	triangles.push_back(Triangle(n, cordinates[0], cordinates[1], cordinates[4], direction));	
	triangles.push_back(Triangle(n, cordinates[5], cordinates[1], cordinates[4], direction));

	n[0]=0;	n[1]=1;	n[2]=0;
	triangles.push_back(Triangle(n, cordinates[2], cordinates[3], cordinates[6], direction));
	triangles.push_back(Triangle(n, cordinates[7], cordinates[3], cordinates[6], direction));


	n[0]=0;	n[1]=0;	n[2]=-1;
	triangles.push_back(Triangle(n, cordinates[0], cordinates[2], cordinates[4], direction));
	triangles.push_back(Triangle(n, cordinates[6], cordinates[2], cordinates[4], direction));

	n[0]=0;	n[1]=0;	n[2]=1;
	triangles.push_back(Triangle(n, cordinates[1], cordinates[3], cordinates[5], direction));	
	triangles.push_back(Triangle(n, cordinates[7], cordinates[3], cordinates[5], direction));
}

int Module::readTrianglesFromStl(char * filepath)
{
    char * solid = "solid";
    char * endsolid = "endsolid";

    char * facet = "facet normal";
    char * endfacet = "endfacet";

    char * loop = "outer loop";
    char * endloop = "endloop";

    char * vertex_str = "vertex";

    char buf[160];

    FILE * f = fopen(filepath, "r");

    if (!f)
	{		
		Logger::InsertErrorMessage("Blad: nie istnieje plik " + gcnew System::String(filepath));		
        return -1;
    }

    float n[3], x[3], y[3], z[3];

    // count = 0;

    int vertexCount=0;
    int trianglesCount=0;

    // parsowanie pliku stl line-by-line
    while (fgets(buf, 160, f))
    {
        // wskaznik roboczy
        char *bufWork = buf;

        // usuniecie poprzedzajacych spacji i tabulacji
        while (*(bufWork) == ' ' || *(bufWork) == '\t')
			bufWork++;
        
        // sprawdzanie "sygnatury" linii
        if (!strncmp(bufWork, vertex_str, strlen(vertex_str)))
        {

			if (vertexCount>2)
			{               
				Logger::InsertErrorMessage("Blad: Vertex Count!");
			}
			fflush(stdout);

            // usuniecie znakow niebedacych cyframi
            while (!isdigit(*(bufWork++)));
            bufWork -= 2;

            sscanf(bufWork, "%e %e %e", &x[vertexCount], &y[vertexCount], &z[vertexCount]);
			//x[vertexCount]+=10*n[0];
			//y[vertexCount]+=10*n[1];
			//z[vertexCount]+=10*n[2];

            vertexCount++;

        } else if (!strncmp(bufWork, solid, strlen(solid)))
            ;//puts("START");
        else if (!strncmp(bufWork, endsolid, strlen(endsolid)))
            ;//puts("STOP");
        else if (!strncmp(bufWork, facet, strlen(facet)))
        {
            vertexCount=0;
            // usuniecie znakow niebedacych cyframi
            while (!isdigit(*(bufWork++)));
            bufWork -= 2;

            sscanf(bufWork, "%e %e %e", &n[0], &n[1], &n[2]);
        } else if (!strncmp(bufWork, endfacet, strlen(endfacet)))
        {
            //puts("EF");
            triangles.push_back(Triangle(n,x,y,z));
            trianglesCount++;
        }
        else if (!strncmp(bufWork, loop, strlen(loop)))
            ;//puts("LOOP");
        else if (!strncmp(bufWork, endloop, strlen(endloop)))
            ;//puts("EndLOOP");
		else {            
			Logger::InsertWarningMessage("nierozpoznana linia: "+gcnew System::String(filepath) + ": " + gcnew System::String(bufWork));
		}

    }

    fclose(f);    
	Logger::InsertInfoMessage("wczytano plik: "+gcnew System::String(filepath) +" - "+trianglesCount+" trojkatow");
    return trianglesCount;
}
void Module::drawCubedModule(){
	glBegin(GL_QUADS);

	int vect[] = {0,3,2,1,4,7,6,5,4,5,1,0,7,6,2,3,5,6,2,1,4,7,3,0};
	for (int i = 23; i >= 0; i--)
		glVertex3f(cModule.vertexes[vect[i]].x, cModule.vertexes[vect[i]].y, cModule.vertexes[vect[i]].z);

	glEnd();
}
void Module::drawModule()
{
    glBegin(GL_TRIANGLES); // Drawing Using Triangles
    
    unsigned int i;

    for (i=0;i<triangles.size();i++)
    {
        #ifdef LIGHT1
        glColor3f(triangles[i].normal.x, triangles[i].normal.y, triangles[i].normal.z);
        #endif
        glNormal3f(triangles[i].normal.x, triangles[i].normal.y, triangles[i].normal.z);

        glVertex3f(triangles[i].vertex[0].x, triangles[i].vertex[0].y, triangles[i].vertex[0].z);
        glVertex3f(triangles[i].vertex[1].x, triangles[i].vertex[1].y, triangles[i].vertex[1].z);
        glVertex3f(triangles[i].vertex[2].x, triangles[i].vertex[2].y, triangles[i].vertex[2].z);

    }


    // error checking?
    GLenum error;
    while ((error = glGetError()) != GL_NO_ERROR)
    {
        //    const GLubyte errorStr;
        //    errorStr = gluErrorString (error);        
		Logger::InsertErrorMessage("błąd OpenGL nr "+ error);        
    }

    glEnd();

}
void Module::createCubedModule(){
	for (unsigned int i = 0; i < this->triangles.size(); i++){
		cModule.analyseTriangle(triangles[i], i == 0? true : false);
	}
	// na podstawie przeanalizowanych powyzej trojkatow tworzymy charakterystyczne punkty prostopadloscianu
	cModule.calculateCubePoints();

}

void Module::addTriangles(Module * module)
{
    unsigned int i;
    for (i=0;i<module->triangles.size();i++)
    {
        this->triangles.push_back(module->triangles[i]);
    }
}

void Module::transform(double Rprev[3][3], double Tprev[3])
{
    transform(this->X, this->Y, this->Z, this->axis, this->R, Rprev, Tprev, 1);
}

void Module::transform(float X, float Y, float Z, float axis, float R, double Rprev[3][3], double Tprev[3], int whichTriangles)
{
    unsigned int i;
    float sinVal, cosVal;
    float x0, y0, z0;
	
	
	sinVal = sin((float)(R * M_PI / 180.0f));
    cosVal = cos((float)(R * M_PI / 180.0f));

    if (axis=='x') // translacja wokol osi X
    {
        Rmat[0][0] = Rprev[0][0];
        Rmat[0][1] = Rprev[0][1] * cosVal + Rprev[0][2] * sinVal;
        Rmat[0][2] = -Rprev[0][1] * sinVal + Rprev[0][2] * cosVal;        

        Rmat[1][0] = Rprev[1][0];
        Rmat[1][1] = Rprev[1][1] * cosVal + Rprev[1][2] * sinVal;
        Rmat[1][2] = -Rprev[1][1] * sinVal + Rprev[1][2] * cosVal;        

        Rmat[2][0] = Rprev[2][0];
        Rmat[2][1] = Rprev[2][1] * cosVal + Rprev[2][2] * sinVal;
        Rmat[2][2] = -Rprev[2][1] * sinVal + Rprev[2][2] * cosVal;      
    }

    else if (axis=='y') // translacja wokol osi Y
    {
        Rmat[0][0] = Rprev[0][0] * cosVal - Rprev[0][2] * sinVal;
        Rmat[0][1] = Rprev[0][1];
        Rmat[0][2] = Rprev[0][0] * sinVal + Rprev[0][2] * cosVal;

        Rmat[1][0] = Rprev[1][0] * cosVal - Rprev[1][2] * sinVal;
        Rmat[1][1] = Rprev[1][1];
        Rmat[1][2] = Rprev[1][0] * sinVal + Rprev[1][2] * cosVal;

        Rmat[2][0] = Rprev[2][0] * cosVal - Rprev[2][2] * sinVal;
        Rmat[2][1] = Rprev[2][1];
        Rmat[2][2] = Rprev[2][0] * sinVal + Rprev[2][2] * cosVal;
    }

    else // translacja wokol osi Z
    {
        Rmat[0][0] = Rprev[0][0] * cosVal + Rprev[0][1] * sinVal;
        Rmat[0][1] = -Rprev[0][0] * sinVal + Rprev[0][1] * cosVal;
        Rmat[0][2] = Rprev[0][2];
        
        Rmat[1][0] = Rprev[1][0] * cosVal + Rprev[1][1] * sinVal;
        Rmat[1][1] = -Rprev[1][0] * sinVal + Rprev[1][1] * cosVal;
        Rmat[1][2] = Rprev[1][2];
        
        Rmat[2][0] = Rprev[2][0] * cosVal + Rprev[2][1] * sinVal;
        Rmat[2][1] = -Rprev[2][0] * sinVal + Rprev[2][1] * cosVal;
        Rmat[2][2] = Rprev[2][2];       
    }

	Tmat[0] = Tprev[0] + Rprev[0][0]*X + Rprev[0][1]*Y + Rprev[0][2]*Z;
	Tmat[1] = Tprev[1] + Rprev[1][0]*X + Rprev[1][1]*Y + Rprev[1][2]*Z;
	Tmat[2] = Tprev[2] + Rprev[2][0]*X + Rprev[2][1]*Y + Rprev[2][2]*Z;

    for (i = 0; i < triangles.size(); i++)
    {
        int j;
        for (j = 0; j < 3; j++)
        {

            x0 = triangles[i].vertex0[j].x;
            y0 = triangles[i].vertex0[j].y;
            z0 = triangles[i].vertex0[j].z;


            if (whichTriangles==0)
            {
                triangles[i].vertex0[j].x = (float)(Rmat[0][0] * x0 + Rmat[0][1] * y0 + Rmat[0][2] * z0 + Tmat[0]);
                triangles[i].vertex0[j].y = (float)(Rmat[1][0] * x0 + Rmat[1][1] * y0 + Rmat[1][2] * z0 + Tmat[1]);
                triangles[i].vertex0[j].z = (float)(Rmat[2][0] * x0 + Rmat[2][1] * y0 + Rmat[2][2] * z0 + Tmat[2]);
            }
            else
            {
                triangles[i].vertex[j].x = (float)(Rmat[0][0] * x0 + Rmat[0][1] * y0 + Rmat[0][2] * z0 + Tmat[0]);
                triangles[i].vertex[j].y = (float)(Rmat[1][0] * x0 + Rmat[1][1] * y0 + Rmat[1][2] * z0 + Tmat[1]);
                triangles[i].vertex[j].z = (float)(Rmat[2][0] * x0 + Rmat[2][1] * y0 + Rmat[2][2] * z0 + Tmat[2]);
            }
        }

        float nx=triangles[i].normal0.x;
        float ny=triangles[i].normal0.y;
        float nz=triangles[i].normal0.z;

        if (whichTriangles==0)
        {
            triangles[i].normal0.x = (float)(Rmat[0][0] * nx + Rmat[0][1] * ny + Rmat[0][2] * nz);
            triangles[i].normal0.y = (float)(Rmat[1][0] * nx + Rmat[1][1] * ny + Rmat[1][2] * nz);
            triangles[i].normal0.z = (float)(Rmat[2][0] * nx + Rmat[2][1] * ny + Rmat[2][2] * nz);
        }
        else
        {
            triangles[i].normal.x = (float)(Rmat[0][0] * nx + Rmat[0][1] * ny + Rmat[0][2] * nz);
            triangles[i].normal.y = (float)(Rmat[1][0] * nx + Rmat[1][1] * ny + Rmat[1][2] * nz);
            triangles[i].normal.z = (float)(Rmat[2][0] * nx + Rmat[2][1] * ny + Rmat[2][2] * nz);
        }

    }
	typedef unsigned int uint;
	for (uint i = 0; i < 8; i++){
		x0 = cModule.vertexes0[i].x;
        y0 = cModule.vertexes0[i].y;
        z0 = cModule.vertexes0[i].z;

        if (whichTriangles==0)
        {
            cModule.vertexes0[i].x = (float)(Rmat[0][0] * x0 + Rmat[0][1] * y0 + Rmat[0][2] * z0 + Tmat[0]);
            cModule.vertexes0[i].y = (float)(Rmat[1][0] * x0 + Rmat[1][1] * y0 + Rmat[1][2] * z0 + Tmat[1]);
            cModule.vertexes0[i].z = (float)(Rmat[2][0] * x0 + Rmat[2][1] * y0 + Rmat[2][2] * z0 + Tmat[2]);
        }
        else
        {
            cModule.vertexes[i].x = (float)(Rmat[0][0] * x0 + Rmat[0][1] * y0 + Rmat[0][2] * z0 + Tmat[0]);
            cModule.vertexes[i].y = (float)(Rmat[1][0] * x0 + Rmat[1][1] * y0 + Rmat[1][2] * z0 + Tmat[1]);
            cModule.vertexes[i].z = (float)(Rmat[2][0] * x0 + Rmat[2][1] * y0 + Rmat[2][2] * z0 + Tmat[2]);
        }
	}
	// punkt centralny cubedModule
	x0 = cModule.center0.x;
    y0 = cModule.center0.y;
    z0 = cModule.center0.z;
	if (whichTriangles==0)
	{
		cModule.center0.x = (float)(Rmat[0][0] * x0 + Rmat[0][1] * y0 + Rmat[0][2] * z0 + Tmat[0]);
		cModule.center0.y = (float)(Rmat[1][0] * x0 + Rmat[1][1] * y0 + Rmat[1][2] * z0 + Tmat[1]);
		cModule.center0.z = (float)(Rmat[2][0] * x0 + Rmat[2][1] * y0 + Rmat[2][2] * z0 + Tmat[2]);
	}
	else
	{
		cModule.center.x = (float)(Rmat[0][0] * x0 + Rmat[0][1] * y0 + Rmat[0][2] * z0 + Tmat[0]);
		cModule.center.y = (float)(Rmat[1][0] * x0 + Rmat[1][1] * y0 + Rmat[1][2] * z0 + Tmat[1]);
		cModule.center.z = (float)(Rmat[2][0] * x0 + Rmat[2][1] * y0 + Rmat[2][2] * z0 + Tmat[2]);
	}
}
